/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package pt.utl.ist.isr.livetrack.tracker.mht;

import pt.utl.ist.isr.livetrack.tracker.interfaces.BestHypothesisObserver;
import com.google.code.javakbest.Murty;
import eu.anorien.mhl.Event;
import eu.anorien.mhl.Fact;
import eu.anorien.mhl.Factory;
import eu.anorien.mhl.HypothesesManager;
import eu.anorien.mhl.Hypothesis;
import eu.anorien.mhl.MHLService;
import eu.anorien.mhl.Watcher;
import eu.anorien.mhl.generator.GeneratedHypotheses;
import eu.anorien.mhl.generator.GeneratedHypothesis;
import eu.anorien.mhl.generator.HypothesesGenerator;
import eu.anorien.mhl.lisbonimpl.extras.visualization.ClusterVisualizer;
import eu.anorien.mhl.lisbonimpl.extras.visualization.LeafVisualizer;
import eu.anorien.mhl.pruner.Pruner;
import eu.anorien.mhl.pruner.PrunerFactory;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collection;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import java.util.Set;
import org.apache.log4j.Logger;
import pt.utl.ist.isr.livetrack.tracker.Entity;
import pt.utl.ist.isr.livetrack.tracker.mht.events.DeletedTargetEvent;
import pt.utl.ist.isr.livetrack.tracker.mht.events.NewTargetEvent;
import pt.utl.ist.isr.livetrack.tracker.mht.events.TargetInEvent;

/**
 *
 * @author David Miguel Antunes <davidmiguel [ at ] antunes.net>
 */
public class MHTracker<TargetInfo> {

    private static final Logger logger = Logger.getLogger(MHTracker.class);
    private MHLService service;
    private Factory factory;
    private HypothesesManager hm;
    private SimpleWatcher watcher;
    private long time = 0;
    private long targetIdGen = 0;
    private int maxNumLeaves, maxDepth, timeUndetected, bestK;
    private double probUndetected, probNewTarget, probFalseAlarm;
    private int gateSize;
    private double newHistogramWeigth;
    private ClusterVisualizer clusterVisualizer;
    private LeafVisualizer leafVisualizer;

    public MHTracker(
            int maxNumLeaves,
            int maxDepth,
            int timeUndetected,
            int bestK,
            double probUndetected,
            double probNewTarget,
            double probFalseAlarm,
            double probChangeZone,
            int gateSize,
            double newHistogramWeigth,
            int histogramDifferencePenalization,
            boolean showDisplay) {
        this.maxNumLeaves = maxNumLeaves;
        this.maxDepth = maxDepth;
        this.timeUndetected = timeUndetected;
        this.bestK = bestK;
        this.probUndetected = probUndetected;
        this.probNewTarget = probNewTarget;
        this.probFalseAlarm = probFalseAlarm;
        this.gateSize = gateSize;
        this.newHistogramWeigth = newHistogramWeigth;
        EntityInFact.CHANGE_ZONE_PROB = probChangeZone;
        EntityInFact.HISTOGRAM_DIFFERENCE_PENALIZATION = histogramDifferencePenalization;

//        ServiceLoader<MHLService> serviceLoader = ServiceLoader.load(MHLService.class);
//        service = serviceLoader.iterator().next();
        service = new eu.anorien.mhl.lisbonimpl.LisbonMHLImpl();
        factory = service.getFactory();
        hm = factory.newHypothesesManager();

        PrunerFactory prunerFactory = factory.getPrunerFactory();

        hm.setPruner(prunerFactory.newCompositePruner(new Pruner[]{
                    prunerFactory.newBestKPruner(maxNumLeaves),
                    prunerFactory.newTreeDepthPruner(maxDepth)}));

        watcher = new SimpleWatcher();
        hm.register(watcher);

        if (showDisplay) {
            clusterVisualizer = new ClusterVisualizer(hm);
//        clusterVisualizer.getLeafVisualizer().setVisible(true);
//        clusterVisualizer.setVisible(true);
            leafVisualizer = clusterVisualizer.getLeafVisualizer();
        }
    }

    public void newScan(long time, List<TDetection<TargetInfo>> m) {
        this.time = time;
        Map<Set<TDetection<TargetInfo>>, Set<Fact>> map = new HashMap<Set<TDetection<TargetInfo>>, Set<Fact>>();
        Set<Fact> isolatedTargets = new HashSet<Fact>(watcher.getFacts());

        createGroups(m, map, isolatedTargets);

        generateHypForGroups(map);

        generateHypIsolatedTargets(isolatedTargets);
    }

    private void generateHypIsolatedTargets(Set<Fact> isolatedTargets) {
        for (Fact fact : isolatedTargets) {
            HashSet<Fact> reqFacts = new HashSet<Fact>();
            reqFacts.add(fact);
            hm.generateHypotheses(new HypothesesGenerator() {

                public GeneratedHypotheses generate(Set<Event> set, Set<Fact> provFacts) {
                    List<GeneratedHypothesis> generatedHypothesisList = new ArrayList<GeneratedHypothesis>();
                    if (provFacts.isEmpty()) {
                        generatedHypothesisList.add(factory.newGeneratedHypothesis(1, new HashSet<Event>(), new HashSet<Fact>()));
                    } else {
                        HashSet<Fact> newFacts = new HashSet<Fact>();
                        HashSet<Event> newEvents = new HashSet<Event>();
                        EntityInFact<TargetInfo> target = (EntityInFact<TargetInfo>) provFacts.iterator().next();
                        if (time - target.getLastDetection() <= timeUndetected || !target.getZone().isExitZone()) {
                            EntityInFact<TargetInfo> targetUpdate = new EntityInFact<TargetInfo>(
                                    target.getEntity(),
                                    target.getZone(),
                                    target.getFeatures(),
                                    target.getEntityInformation(),
                                    target.getLastDetection());
                            newFacts.add(targetUpdate);
                            TargetInEvent<TargetInfo> targetInEvent = new TargetInEvent<TargetInfo>(
                                    time, targetUpdate.getEntity(),
                                    targetUpdate.getZone(), targetUpdate.getEntityInformation());
                            newEvents.add(targetInEvent);
                        } else {
                            DeletedTargetEvent deletedTargetEvent = new DeletedTargetEvent(
                                    time, target.getEntity());
                            newEvents.add(deletedTargetEvent);
                        }
                        generatedHypothesisList.add(factory.newGeneratedHypothesis(probUndetected, newEvents, newFacts));
                    }
                    return factory.newGeneratedHypotheses(generatedHypothesisList);
                }
            }, new HashSet<Event>(), reqFacts);
        }
    }

    private void generateHypForGroups(Map<Set<TDetection<TargetInfo>>, Set<Fact>> map) {
        for (final Map.Entry<Set<TDetection<TargetInfo>>, Set<Fact>> entry : map.entrySet()) {
            hm.generateHypotheses(new HypothesesGenerator() {

                private List<int[]> solveAssignment(double[][] costMatrix) {
                    double[][] costMatrix2 = new double[costMatrix.length][costMatrix[0].length];
                    for (int i = 0; i < costMatrix.length; i++) {
                        for (int j = 0; j < costMatrix[0].length; j++) {
                            costMatrix2[i][j] = -Math.log(0.9 * costMatrix[i][j] + 0.1);
                        }
                    }
                    List<int[]> result = Murty.solve(costMatrix2, bestK);
                    return result;
                }

                @Override
                public GeneratedHypotheses generate(Set<Event> set, Set<Fact> provFacts) {
                    List<TDetection<TargetInfo>> detections = new ArrayList<TDetection<TargetInfo>>(entry.getKey());
                    List<Fact> targets = new ArrayList<Fact>(provFacts);
                    double[][] costMatrix = new double[detections.size()][targets.size() + detections.size() * 2];
                    for (int i = 0; i < detections.size(); i++) {
                        TDetection detection = detections.get(i);
                        for (int j = 0; j < targets.size(); j++) {
                            costMatrix[i][j] = ((EntityInFact) targets.get(j)).detectionProbability(detection);
                        }
                        costMatrix[i][targets.size() + i] = probFalseAlarm;
                        costMatrix[i][targets.size() + detections.size() + i] = detection.getZone().isEntryZone() ? probNewTarget : 0;
                    }
                    List<GeneratedHypothesis> generatedHypothesesList = new ArrayList<GeneratedHypothesis>();
                    List<int[]> result = solveAssignment(costMatrix);
                    for (int solution = 0; solution < result.size(); solution++) {
                        int[] assignments = result.get(solution);
                        double hypProb = 1;
                        Set<Fact> newFacts = new HashSet<Fact>();
                        Set<Event> newEvents = new HashSet<Event>();
                        for (int detectionNumber = 0; detectionNumber < assignments.length; detectionNumber++) {
                            int assignment = assignments[detectionNumber];
                            TDetection<TargetInfo> detection = detections.get(detectionNumber);
                            hypProb *= costMatrix[detectionNumber][assignment];
                            if (assignment < targets.size()) {
                                EntityInFact<TargetInfo> target = (EntityInFact<TargetInfo>) targets.get(assignment);
                                EntityInFact<TargetInfo> targetUpdate = new EntityInFact<TargetInfo>(
                                        target.getEntity(),
                                        detection.getZone(),
                                        weightFeatures(target.getFeatures(), detection.getFeatures()),
                                        detection.getTargetInfo(),
                                        time);
                                newFacts.add(targetUpdate);

                                TargetInEvent targetInEvent = new TargetInEvent(
                                        time, targetUpdate.getEntity(),
                                        targetUpdate.getZone(), targetUpdate.getEntityInformation());
                                newEvents.add(targetInEvent);

                            } else if (assignment < targets.size() + detections.size()) {
                                // False Alarm
                            } else {
                                // New Target
                                EntityInFact<TargetInfo> newTarget = new EntityInFact<TargetInfo>(
                                        new Entity(targetIdGen++),
                                        detection.getZone(),
                                        detection.getFeatures(),
                                        detection.getTargetInfo(),
                                        time);
                                newFacts.add(newTarget);

                                TargetInEvent targetInEvent = new TargetInEvent(
                                        time, newTarget.getEntity(),
                                        newTarget.getZone(), newTarget.getEntityInformation());
                                newEvents.add(targetInEvent);

                                NewTargetEvent newTargetEvent = new NewTargetEvent(time, newTarget.getEntity());
                                newEvents.add(newTargetEvent);
                            }
                        }
                        int[] item2Customer = new int[costMatrix[0].length];

                        Arrays.fill(item2Customer, -1);
                        for (int i = 0; i < assignments.length; i++) {
                            item2Customer[assignments[i]] = i;
                        }

                        for (int i = 0; i < targets.size(); i++) {
                            if (item2Customer[i] == -1) {
                                EntityInFact<TargetInfo> target = (EntityInFact<TargetInfo>) targets.get(i);
                                if (time - target.getLastDetection() <= timeUndetected || !target.getZone().isExitZone()) {
                                    EntityInFact<TargetInfo> targetUpdate = new EntityInFact<TargetInfo>(
                                            target.getEntity(),
                                            target.getZone(),
                                            target.getFeatures(),
                                            target.getEntityInformation(),
                                            target.getLastDetection());
                                    newFacts.add(targetUpdate);
                                    TargetInEvent targetInEvent = new TargetInEvent(
                                            time, targetUpdate.getEntity(),
                                            targetUpdate.getZone(), targetUpdate.getEntityInformation());
                                    newEvents.add(targetInEvent);
                                } else {
                                    DeletedTargetEvent deletedTargetEvent = new DeletedTargetEvent(
                                            time, target.getEntity());
                                    newEvents.add(deletedTargetEvent);
                                }
                                hypProb *= probUndetected;
                            }
                        }
                        if (hypProb > 0) {
                            generatedHypothesesList.add(factory.newGeneratedHypothesis(hypProb, newEvents, newFacts));
                        }
                    }
                    if (generatedHypothesesList.isEmpty()) {
                        throw new RuntimeException("No feasible hypothesis");
                    }
                    return factory.newGeneratedHypotheses(generatedHypothesesList);
                }
            }, new HashSet<Event>(), entry.getValue());
        }
    }

    private void createGroups(List<TDetection<TargetInfo>> m, Map<Set<TDetection<TargetInfo>>, Set<Fact>> map, Set<Fact> isolatedTargets) {
        {
            for (TDetection detection : m) {
                HashSet<Fact> targets = new HashSet<Fact>();
                HashSet<TDetection<TargetInfo>> detections = new HashSet<TDetection<TargetInfo>>();
                detections.add(detection);
                map.put(detections, targets);
                for (Fact fact : watcher.getFacts()) {
                    EntityInFact target = (EntityInFact) fact;
                    if (target.detectionInGate(detection)) {
                        targets.add(fact);
                        isolatedTargets.remove(fact);
                    }
                }
            }
            outter:
            while (true) {
                for (Map.Entry<Set<TDetection<TargetInfo>>, Set<Fact>> e1 : map.entrySet()) {
                    for (Map.Entry<Set<TDetection<TargetInfo>>, Set<Fact>> e2 : map.entrySet()) {
                        if (!e1.equals(e2)) {
                            if (interset(e1.getValue(), e2.getValue())) {
                                HashSet<Fact> newFacts = new HashSet<Fact>();
                                newFacts.addAll(e1.getValue());
                                newFacts.addAll(e2.getValue());
                                HashSet<TDetection<TargetInfo>> newDetections = new HashSet<TDetection<TargetInfo>>();
                                newDetections.addAll(e1.getKey());
                                newDetections.addAll(e2.getKey());
                                map.put(newDetections, newFacts);
                                map.remove(e1.getKey());
                                map.remove(e2.getKey());
                                continue outter;
                            }
                        }
                    }
                }
                break;
            }
        }
    }

    private boolean interset(Set s1, Set s2) {
        for (Object object : s2) {
            if (s1.contains(object)) {
                return true;
            }
        }
        for (Object object : s1) {
            if (s2.contains(object)) {
                return true;
            }
        }
        return false;
    }

    public Hypothesis getBestHypothesis() {
        return hm.getBestHypothesis();
    }

    public Map<Long, TargetInfo> getBestHypothesisTargets() {
        Map<Long, TargetInfo> targets = new HashMap<Long, TargetInfo>();
        for (EntityInFact<TargetInfo> fact : (Set<EntityInFact<TargetInfo>>) (Object) hm.getBestHypothesis().getFacts().keySet()) {
            targets.put(fact.getEntity().getId(), fact.getEntityInformation());
        }
        return targets;
    }

    private double[] weightFeatures(double[] oldH, double[] newH) {
        double[] rslt = new double[oldH.length];
        for (int i = 0; i < oldH.length; i++) {
            rslt[i] = newHistogramWeigth * newH[i] + (1 - newHistogramWeigth) * oldH[i];
        }
        return rslt;
    }

    public void registerBestHypothesisObserver(final BestHypothesisObserver o) {
        hm.register(new Watcher() {

            @Override
            public void newFact(Fact fact) {
            }

            @Override
            public void newFacts(Collection<Fact> clctn) {
            }

            @Override
            public void removedFact(Fact fact) {
            }

            @Override
            public void removedFacts(Collection<Fact> clctn) {
            }

            @Override
            public void newEvent(Event event) {
            }

            @Override
            public void newEvents(Collection<Event> clctn) {
            }

            @Override
            public void removedEvent(Event event) {
            }

            @Override
            public void removedEvents(Collection<Event> clctn) {
            }

            @Override
            public void confirmedEvent(Event event) {
            }

            @Override
            public void confirmedEvents(Collection<Event> clctn) {
            }

            @Override
            public void bestHypothesis(Hypothesis hpths) {
                o.newBestHypothesis(hpths);
            }
        });
    }

    public ClusterVisualizer getClusterVisualizer() {
        return clusterVisualizer;
    }

    public LeafVisualizer getLeafVisualizer() {
        return leafVisualizer;
    }

    public void registerWatcher(Watcher watcher) {
        hm.register(watcher);
    }
}
